#include "vehicle_control_handle/VehicleControlHandle.h"

using namespace std::chrono_literals;

VehicleControlHandle::VehicleControlHandle():Node("VehicleControlHandle")
{

    this->__Init();
    // this->__XBoxOpen("/dev/input/js0");

    // 开读手柄的线程 >>>>>
    std::thread thread1_(std::bind(&VehicleControlHandle::__ThreadReadXBox, this));
    thread1_.detach();

    this->__rpcVehicleControlPublisher = 
        this->create_publisher<carla_ros2_interface::msg::RpcVehicleControl>(
            "rpc_vehicle_control", 
            10);
    this->__timer50ms = 
        this->create_wall_timer(
            50ms, 
            std::bind(&VehicleControlHandle::__PublishRpcVehcileControl, 
            this));
}

void VehicleControlHandle::__PublishRpcVehcileControl()
{
    this->__rpcVehicleControlPublisher->publish(this->__rpcVehicleControlMsg);
}

VehicleControlHandle::~VehicleControlHandle()
{
    this->__isRunning = false;
    // this->__XBoxMapClose();
}

void VehicleControlHandle::__XBoxOpen(const char* file_name)
{
    // int xbox_fd;  

    this->__xboxFd = open(file_name, O_RDONLY);
    // std::fstream f_ = std::fstream()
    if (this->__xboxFd < 0)  {  
        // perror("open");
        RCLCPP_ERROR(rclcpp::get_logger("__XBoxOpen"), "打开手柄失败");
        throw("打开手柄失败");
    }
}

int VehicleControlHandle::__xBoxMapRead()
{
    int len, type, number, value;  
    struct js_event js;  

    len = read(this->__xboxFd, &js, sizeof(struct js_event));  
    if (len < 0)  
    {  
        // perror("read");
        // return -1;
        RCLCPP_ERROR(rclcpp::get_logger("__xBoxMapRead"), "读取手柄状态失败");
        return -1;
    }  

    type = js.type;  
    number = js.number;  
    value = js.value;  

    this->__xBoxMap.time = js.time;  

    if (type == JS_EVENT_BUTTON)  
    {  
        switch (number)  
        {  
            case XBOX_BUTTON_A:  
                this->__xBoxMap.a = value;  
                break;  

            case XBOX_BUTTON_B:  
                this->__xBoxMap.b = value;  
                break;  

            case XBOX_BUTTON_X:  
                this->__xBoxMap.x = value;  
                break;  

            case XBOX_BUTTON_Y:  
                this->__xBoxMap.y = value;  
                break;  

            case XBOX_BUTTON_LB:  
                this->__xBoxMap.lb = value;
                // 倒挡切换
                if (value == 1) {
                    this->__rpcVehicleControlMsg.reverse = !this->__rpcVehicleControlMsg.reverse;
                }
                // RCLCPP_INFO(rclcpp::get_logger("XBOX_BUTTON_LB"), " %d", value);
                break;  

            case XBOX_BUTTON_RB:  
                this->__xBoxMap.rb = value;  
                break;  

            case XBOX_BUTTON_START:  
                this->__xBoxMap.start = value;  
                break;  

            case XBOX_BUTTON_BACK:  
                this->__xBoxMap.back = value;  
                break;  

            case XBOX_BUTTON_HOME:  
                this->__xBoxMap.home = value;  
                break;  

            case XBOX_BUTTON_LO:  
                this->__xBoxMap.lo = value;  
                break;  

            case XBOX_BUTTON_RO:  
                this->__xBoxMap.ro = value;  
                break;  

            default:  
                break;  
        }  
    }  
    else if (type == JS_EVENT_AXIS)  
    {  
        switch(number)  
        {  
            case XBOX_AXIS_LX:  
                this->__xBoxMap.lx = value;
                this->__rpcVehicleControlMsg.steer = float(value) / 32767;
                break;  

            case XBOX_AXIS_LY:  
                this->__xBoxMap.ly = value;  
                break;  

            case XBOX_AXIS_RX:  
                this->__xBoxMap.rx = value;  
                break;  

            case XBOX_AXIS_RY:  
                this->__xBoxMap.ry = value;  
                break;  

            case XBOX_AXIS_LT:  
                this->__xBoxMap.lt = value;
                this->__rpcVehicleControlMsg.brake = (32767 + float(value)) / 65534;
                break;  

            case XBOX_AXIS_RT:  
                this->__xBoxMap.rt = value;
                this->__rpcVehicleControlMsg.throttle = (32767 + float(value)) / 65534;
                break;  

            case XBOX_AXIS_XX:  
                this->__xBoxMap.xx = value;  
                break;  

            case XBOX_AXIS_YY:  
                this->__xBoxMap.yy = value;  
                break;  

            default:  
                break;  
        }  
    }  
    else  
    {  
        /* Init do nothing */  
    }  

    return len;
    // return;
}

void VehicleControlHandle::__XBoxMapClose()
{
    if (this->__xboxFd != 0) {
        close(this->__xboxFd);
    }  
    return;  
}

void VehicleControlHandle::__Init()
{
    this->__rpcVehicleControlMsg.brake = 0.0f;
    this->__rpcVehicleControlMsg.gear = 0;
    this->__rpcVehicleControlMsg.hand_brake = false;
    this->__rpcVehicleControlMsg.manual_gear_shift = false;
    this->__rpcVehicleControlMsg.reverse = false;
    this->__rpcVehicleControlMsg.steer = 0.0f;
    this->__rpcVehicleControlMsg.throttle = 0.0f;
}

void VehicleControlHandle::__ThreadReadXBox()
{
    int len_ = 0;
    this->__XBoxOpen("/dev/input/js0");

    this->__isRunning = true;
    while (true)
    {
        if (!this->__isRunning) {
            this->__XBoxMapClose();
            break;
        }
        len_ = __xBoxMapRead();
        if (len_ <= 0) {
            std::this_thread::sleep_for(5ms);
            continue;
        }
        /* code */
    }
    
}